#include "MATRIX3.H"

Matrix3 identityMatrix3 () {
	Matrix3 retVal;
	retVal.M[0][0] = 1;
	retVal.M[0][1] = 0;
	retVal.M[0][2] = 0;
	retVal.M[1][0] = 0;
	retVal.M[1][1] = 1;
	retVal.M[1][2] = 0;
	retVal.M[2][0] = 0;
	retVal.M[2][1] = 0;
	retVal.M[2][2] = 1;
	return retVal;
}
boolean isEqual(Matrix3 left, Matrix3 right){	
	int i,j;
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			if((float)fabs(left.M[i][j]-right.M[i][j]) > FLT_EPSILON)
			{
				return FALSE;
			}
		}
	}
	return TRUE;
}
float determinantMatrix3 (Matrix3 mat) {
	return mat.M[0][0] * mat.M[1][1] * mat.M[2][2] + 
		mat.M[0][1] * mat.M[1][2] * mat.M[2][0] + 
		mat.M[0][2] * mat.M[1][0] * mat.M[2][1] - 
		mat.M[0][0] * mat.M[1][2] * mat.M[2][1] - 
		mat.M[0][1] * mat.M[1][0] * mat.M[2][2] - 
		mat.M[0][2] * mat.M[1][1] * mat.M[2][0];
}
Matrix3 negateMatrix3(Matrix3 mat){
	Matrix3 retVal;
	int i,j;
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			retVal.M[i][j] = -(mat.M[i][j]);
		}
	}
	return retVal;
}
Matrix3 scalingMatrix3 (float sx, float sy) {
	Matrix3 retVal;
	retVal.M[0][0] = sx;
	retVal.M[0][1] = 0;
	retVal.M[0][2] = 0;
	retVal.M[1][0] = 0;
	retVal.M[1][1] = sy;
	retVal.M[1][2] = 0;
	retVal.M[2][0] = 0;
	retVal.M[2][1] = 0;
	retVal.M[2][2] = 1;
	return retVal;
}
Matrix3 rotationMatrix3 (float angle) {	
	Matrix3 retVal;	
	float vCos, vSin;	
	angle =  angle * M_PI/180;	
	vCos = (float)cos(angle);
	vSin = (float)sin(angle);	
	retVal.M[0][0] = vCos;
	retVal.M[0][1] = -vSin;
	retVal.M[0][2] = 0;
	retVal.M[1][0] = vSin;
	retVal.M[1][1] = vCos;
	retVal.M[1][2] = 0;
	retVal.M[2][0] = 0;
	retVal.M[2][1] = 0;
	retVal.M[2][2] = 1;
	return retVal;
}
Matrix3 translationMatrix3 (float x, float y) {
	Matrix3 retVal;	
	retVal.M[0][0] = 1;
	retVal.M[0][1] = 0;
	retVal.M[0][2] = x;
	retVal.M[1][0] = 0;
	retVal.M[1][1] = 1;
	retVal.M[1][2] = y;
	retVal.M[2][0] = 0;
	retVal.M[2][1] = 0;
	retVal.M[2][2] = 1;
	return retVal;
}
boolean inverseMatrix3 (Matrix3 mat, Matrix3* retVal) {
	float det = determinantMatrix3(mat);
	if (det == 0) return FALSE;	
	retVal->M[0][0] = (mat.M[1][1]*mat.M[2][2]-mat.M[1][2]*mat.M[2][1])/det;
	retVal->M[0][1] = (mat.M[0][2]*mat.M[2][1]-mat.M[0][1]*mat.M[2][2])/det;
	retVal->M[0][2] = (mat.M[0][1]*mat.M[1][2]-mat.M[0][2]*mat.M[1][1])/det;
	retVal->M[1][0] = (mat.M[1][2]*mat.M[2][0]-mat.M[1][0]*mat.M[2][2])/det;
	retVal->M[1][1] = (mat.M[0][0]*mat.M[2][2]-mat.M[0][2]*mat.M[2][0])/det;
	retVal->M[1][2] = (mat.M[0][2]*mat.M[1][0]-mat.M[0][0]*mat.M[1][2])/det;
	retVal->M[2][0] = (mat.M[1][0]*mat.M[2][1]-mat.M[1][1]*mat.M[2][0])/det;
	retVal->M[2][1] = (mat.M[2][0]*mat.M[0][1]-mat.M[0][0]*mat.M[2][1])/det;
	retVal->M[2][2] = (mat.M[0][0]*mat.M[1][1]-mat.M[0][1]*mat.M[1][0])/det;
	return TRUE;
}
Matrix3 addMatrix3(Matrix3 mat, Matrix3 adder){
	Matrix3 retVal;
	int i,j;
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			retVal.M[i][j] = mat.M[i][j] + adder.M[i][j];
		}
	}
	return retVal;
}
Matrix3 subMatrix3(Matrix3 mat, Matrix3 subber){
	Matrix3 retVal;
	int i,j;
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			retVal.M[i][j] = mat.M[i][j]-subber.M[i][j];
		}
	}
	return retVal;
}
Matrix3 scaleMatrix3(Matrix3 mat, float scale){
	Matrix3 retVal;
	int i,j;
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			retVal.M[i][j] = mat.M[i][j]*scale;
		}
	}
	return retVal;
}
Matrix3 mulMatrix3 (Matrix3 mat1, Matrix3 mat2) {
	Matrix3 retVal;
	retVal.M[0][0] = mat1.M[0][0] * mat2.M[0][0] + mat1.M[0][1] * mat2.M[1][0] + mat1.M[0][2] * mat2.M[2][0]; 
	retVal.M[0][1] = mat1.M[0][0] * mat2.M[0][1] + mat1.M[0][1] * mat2.M[1][1] + mat1.M[0][2] * mat2.M[2][1]; 
	retVal.M[0][2] = mat1.M[0][0] * mat2.M[0][2] + mat1.M[0][1] * mat2.M[1][2] + mat1.M[0][2] * mat2.M[2][2];	
	retVal.M[1][0] = mat1.M[1][0] * mat2.M[0][0] + mat1.M[1][1] * mat2.M[1][0] + mat1.M[1][2] * mat2.M[2][0];	
	retVal.M[1][1] = mat1.M[1][0] * mat2.M[0][1] + mat1.M[1][1] * mat2.M[1][1] + mat1.M[1][2] * mat2.M[2][1];
	retVal.M[1][2] = mat1.M[1][0] * mat2.M[0][2] + mat1.M[1][1] * mat2.M[1][2] + mat1.M[1][2] * mat2.M[2][2];
	retVal.M[2][0] = mat1.M[2][0] * mat2.M[0][0] + mat1.M[2][1] * mat2.M[1][0] + mat1.M[2][2] * mat2.M[2][0];		
	retVal.M[2][1] = mat1.M[2][0] * mat2.M[0][1] + mat1.M[2][1] * mat2.M[1][1] + mat1.M[2][2] * mat2.M[2][1];		
	retVal.M[2][2] = mat1.M[2][0] * mat2.M[0][2] + mat1.M[2][1] * mat2.M[1][2] + mat1.M[2][2] * mat2.M[2][2];
	return retVal;
}
Matrix3 divMatrix3(Matrix3 mat, float divisor){
	Matrix3 retVal = identityMatrix3();
	if(divisor != 0)
	{
		int i,j;
		for(i=0;i<3;i++)
		{
			for(j=0;j<3;j++)
			{
				retVal.M[i][j] = mat.M[i][j]/divisor;
			}
		}
	}
	return retVal;
}
boolean divideMatrix3(Matrix3 mat, Matrix3 divisor, Matrix3* retVal){
	Matrix3 inv;
	boolean value = inverseMatrix3(divisor, &inv);
	if(value)
	{
		*retVal = mulMatrix3(mat, inv);
	}
	else
	{
		*retVal = identityMatrix3();
	}
	return value;
}
Matrix3 copyMatrix3(Matrix3 mat){
	Matrix3 retVal;
	int i,j;
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			retVal.M[i][j] = mat.M[i][j];
		}
	}
	return retVal;
}
